#include "MPC_Matlab_T.h"
#include "../../../ThirdPartyCode/Utilities.h"
#include "../../../ThirdPartyCode/NR_Vector_CVersion.h"
#include "../model.h"

// ----------------------------------------------------------------------
MPC_Matlab_T::MPC_Matlab_T( double	iDens_set,
			double	iFact_D, 
			double	iRough2_set,
			double	iFact_R2, 
			double	iHt_set,	
			double	iFact_H, 
			double  ivarR2_set,
			double  iFact_varR2,
			double  iM2_set,
			double  iM2_fact,
			double	it_end,
			double	iT_ini,
			double	iRT_T,
			double	it_int_ctrl, 
			int		iStep_num,
			int		iRel_err,
			char*	iFileName,
			int		iDetail,
			int		iControllerID,
			char*   iMPCFunctionName):
			MPC_Base(iDens_set,
				     iFact_D,
					 iRough2_set,
					 iFact_R2,
					 iHt_set,
					 iFact_H,
					 it_end,
					 iT_ini,
					 iRT_T,
					 it_int_ctrl,
					 iStep_num,
					 iRel_err,
					 iFileName,
					 iDetail,
					 iControllerID)
{
	try{
		cout << "\n MPC using Matlab engine (MPC_Matlab_T) is used\n";
		
		if (!(ep = engOpen("\0"))) 
		{
			fprintf(stderr, "\nCan't start MATLAB engine\n");
			system("pause");
			exit(0);
		}
		engOutputBuffer(ep,MatlabOutputBuffer,5120);

		engEvalString(ep,"cd('C:\Documents and Settings\Xinyu\Desktop\LPCVD_Develop\Blocks\Controller\MPC_Base')");
		engEvalString(ep,"addpath(genpath('C:\Documents and Settings\Xinyu\Desktop\LPCVD_Develop\Blocks\Controller\MPC_Base'))");
		cout << MatlabOutputBuffer << "\n";
		engEvalString(ep,"clear all");
		engEvalString(ep,"diary MPC_Matlab_T.log");
		
		mxAlphas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
		alphas		= mxGetPr(mxAlphas);
		engPutVariable(ep,"Alphas",mxAlphas);

		mxBetas 	= mxCreateDoubleMatrix(MODE,1,mxREAL);
		betas		= mxGetPr(mxBetas);
		engPutVariable(ep,"Betas",mxBetas);

		mxHt 		= mxCreateDoubleScalar(-1.0);
		pHt			= mxGetPr(mxHt);
		engPutVariable(ep,"Ht",mxHt);

		mxLb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
		pLb			= mxGetPr(mxLb);
		engPutVariable(ep,"Lb",mxLb);

		mxUb		= mxCreateDoubleMatrix(Step_num,1,mxREAL);
		pUb			= mxGetPr(mxUb);
		engPutVariable(ep,"Ub",mxUb);

		mxG_dens 	= mxCreateDoubleScalar(-1.0);
		pG_dens		= mxGetPr(mxG_dens);
		engPutVariable(ep,"G_dens",mxG_dens);
		
		mxW0		= mxCreateDoubleScalar((T_LOW+T_HIGH)/2);
		pW0			= mxGetPr(mxW0);
		engPutVariable(ep,"W0",mxW0);
		
		mxTime		= mxCreateDoubleScalar(0);
		pTime		= mxGetPr(mxTime);
		engPutVariable(ep,"Time",mxTime);
		
		mxD_set		= mxCreateDoubleScalar(iDens_set);
		mxFact_D	= mxCreateDoubleScalar(iFact_D);
		mxR2_set	= mxCreateDoubleScalar(iRough2_set);
		mxFact_R2	= mxCreateDoubleScalar(iFact_R2);
		mxHt_set    = mxCreateDoubleScalar(iHt_set);
		mxFact_H    = mxCreateDoubleScalar(iFact_H);
		mxVarR2_set = mxCreateDoubleScalar(ivarR2_set);
		mxFact_VarR2= mxCreateDoubleScalar(iFact_varR2);
		mxM2_set    = mxCreateDoubleScalar(iM2_set);
		mxM2_fact   = mxCreateDoubleScalar(iM2_fact);
		mxStep_num	= mxCreateDoubleScalar(iStep_num);
		mxMode		= mxCreateDoubleScalar(MODE);
		mxdt		= mxCreateDoubleScalar(it_int_ctrl);
		mxT_LOW	    = mxCreateDoubleScalar(T_LOW);
		mxT_HIGH	= mxCreateDoubleScalar(T_HIGH);
		mxRT_T	    = mxCreateDoubleScalar(iRT_T);
		mxT_end		= mxCreateDoubleScalar(it_end);
		mxControllerTypeID = mxCreateDoubleScalar(ControllerTypeID);
		
		engPutVariable(ep,"D_set",mxD_set);
		engPutVariable(ep,"Fact_D",mxFact_D);
		engPutVariable(ep,"R2_set",mxR2_set);
		engPutVariable(ep,"Fact_R2",mxFact_R2);
		engPutVariable(ep,"Ht_set",mxHt_set);
		engPutVariable(ep,"Fact_H",mxFact_H);
		engPutVariable(ep,"varR2_set",mxVarR2_set);
		engPutVariable(ep,"Fact_varR2",mxFact_VarR2);
		engPutVariable(ep,"M2_set",mxM2_set);
		engPutVariable(ep,"M2_fact",mxM2_fact);
		engPutVariable(ep,"Step_num",mxStep_num);
		engPutVariable(ep,"Mode",mxMode);
		engPutVariable(ep,"dt",mxdt);
		engPutVariable(ep,"T_LOW",mxT_LOW);
		engPutVariable(ep,"T_HIGH",mxT_HIGH);
		engPutVariable(ep,"RT_T",mxRT_T);
		engPutVariable(ep,"T_end",mxT_end);
		engPutVariable(ep,"ControllerTypeID",mxControllerTypeID);
		
		engEvalString(ep,"MPC_Matlab_T_ini");
		cerr << "\nMPC_Matlab_T_ini.m returns:\n" << MatlabOutputBuffer << "\n";
		MPCFunctionName = iMPCFunctionName;
		fout_MPC3.open("MPC_Matlab_T.log");
	}
	catch(const char *s)
	{
		cerr << "\nError at MPC_Matlab_T::MPC_Matlab_T()";
		cerr << "\n" << MatlabOutputBuffer << "\n";
		cerr << "\n" << s;
	}
}
// ----------------------------------------------------------------------
MPC_Matlab_T::~MPC_Matlab_T()
{
	mxDestroyArray(mxAlphas);
	mxDestroyArray(mxBetas);
	mxDestroyArray(mxHt);
//	mxDestroyArray(mxRough);
	mxDestroyArray(mxG_dens);
	mxDestroyArray(mxW0);
	
	mxDestroyArray(mxD_set);
	mxDestroyArray(mxFact_D);
	mxDestroyArray(mxR2_set);
	mxDestroyArray(mxFact_R2);
	mxDestroyArray(mxHt_set);
	mxDestroyArray(mxFact_H);
	mxDestroyArray(mxVarR2_set);
	mxDestroyArray(mxFact_VarR2);
	mxDestroyArray(mxM2_set);
	mxDestroyArray(mxM2_fact);
	mxDestroyArray(mxStep_num);
	mxDestroyArray(mxMode);
	mxDestroyArray(mxdt);
	mxDestroyArray(mxT_LOW);
	mxDestroyArray(mxT_HIGH);
	mxDestroyArray(mxRT_T);
	mxDestroyArray(mxT_end);
	mxDestroyArray(mxControllerTypeID);
	
	engEvalString(ep,"diary MPC_Matlab.log");
	
	engClose(ep);
	fout_MPC3.close();
}
// ----------------------------------------------------------------------
double MPC_Matlab_T::update(struct lattice_state iLState,
					const double P, const double T)
{
	//cout << "\nIn MPC_Matlab_T::update()";
	int i;
	char msgCommand[1024];
	msgCommand[0] = '\0';
	
	mxArray *mxW = NULL;
	double  *pW;
	mxArray *mxT = NULL;
	double  *pT;
	mxArray *mxFval = NULL;
	double  *pFval;
	mxArray *mxHopt = NULL;
	double  *pHopt;
	mxArray *mxDopt = NULL;
	double  *pDopt;
	mxArray *mxR2opt = NULL;
	double  *pR2opt;
	mxArray *mxVarR2opt = NULL;
	double  *pVarR2opt;
	mxArray *mxM2opt = NULL;
	double  *pM2opt;
	mxArray *mxM2_0 = NULL;
	double  *pM2_0;
	
	try{	
		// 1. Pass variable from C to Matlab -------------------------------------
	
		memcpy(alphas,iLState.alphas,MODE*sizeof(double));
		memcpy(betas,iLState.betas,MODE*sizeof(double));
		ht			= iLState.H;
		(*pHt)		= iLState.H;
		R2			= iLState.R2;
		//	(*pRough) = iLState.rough;
		g_dens		= iLState.SOR;
		(*pG_dens)	= iLState.SOR;
		(*pW0)		= dep_rate;
		(*pTime)	= time;

		engPutVariable(ep,"Alphas",mxAlphas);
		engPutVariable(ep,"Betas",mxBetas);
		engPutVariable(ep,"Ht",mxHt);
		//	engPutVariable(ep,"Rough",mxRough);
		engPutVariable(ep,"G_dens",mxG_dens);
		engPutVariable(ep,"W0",mxW0);
		engPutVariable(ep,"Time",mxTime);

		engEvalString(ep,"In.alpha = Alphas");
		engEvalString(ep,"In.beta = Betas");
		engEvalString(ep,"In.rho = G_dens");
		//engEvalString(ep,"In.R2 = R2");
		engEvalString(ep,"In.h = Ht");
		engEvalString(ep,"In.t = Time");
		//cout << "\nMPC_Matlab_T::update(): end of part 1";
	}
	catch(const char *s)
	{
		cerr << "\n" << MatlabOutputBuffer << "\n";
		cerr << "\n" << s;
	}

	// 2. Solve the optimization problem in Matlab ----------------------------
	sprintf(msgCommand,"[T,info,Settings] = MPC_Matlab_T_update(Settings,In,T)");
	try{
		engEvalString(ep,msgCommand);
		//cout << "\n" << MatlabOutputBuffer << "\n";
		//cout << "\nMPC_Matlab_T::update(): end of part 2";
	}
	catch(const char *s) // If there is any error, report 
	{
		cerr << "\n" << msgCommand << "\n";
		cerr << "\n" << MatlabOutputBuffer << "\n";
		engEvalString(ep,"display(State)");
		cerr << "\n" << MatlabOutputBuffer << "\n";
	}
	try{
		// 3. Get variables back to C----------------------------------------
		// cout << "Pass variables from Matlab to C";

		engEvalString(ep,"Topt = info.Uopt");
		//cout << "\nMPC_Matlab_T::update(): end of part 3.1";
		mxT 	= engGetVariable(ep,"Topt");
		//cout << "\nmxT = " << mxT;
		//cout << "\nMPC_Matlab_T::update(): end of part 3.2";
		pT		= mxGetPr(mxT);
		//cout << "\nMPC_Matlab_T::update(): end of part 3.3";
		//cout << "\n" << mxT;
		for(i=0;i<Step_num;i++)
		{
			//v_dep_rate[i+1] = pT[i];
			v_T[i+1] = pT[i];
		}
		//cout << "\nMPC_Matlab_T::update(): end of part 3.4";
		MPC_Matlab_T::T = pT[0];
		mxDestroyArray(mxT);
		//cout << "\nMPC_Matlab_T::update(): end of part 3";
		// 4. Save diagnostic information ----------------------------------
		engEvalString(ep,"cost = info.cost");
		mxFval 	= engGetVariable(ep,"cost");
		pFval   = mxGetPr(mxFval);
		MPC_diag.cost    = pFval[0];
		mxDestroyArray(mxFval);
		engEvalString(ep,"Hopt = info.Hopt");
		mxHopt  = engGetVariable(ep,"Hopt");
		pHopt   = mxGetPr(mxHopt);
		engEvalString(ep,"Dopt = info.Dopt");
		mxDopt  = engGetVariable(ep,"Dopt");
		pDopt   = mxGetPr(mxDopt);
		engEvalString(ep,"R2opt = info.R2opt");
		mxR2opt = engGetVariable(ep,"R2opt");
		pR2opt  = mxGetPr(mxR2opt);
		engEvalString(ep,"varR2opt = info.varR2opt");
		mxVarR2opt = engGetVariable(ep,"varR2opt");
		pVarR2opt  = mxGetPr(mxVarR2opt);
		engEvalString(ep,"M2opt = info.M2opt");
		mxM2opt    = engGetVariable(ep,"M2opt");
		pM2opt     = mxGetPr(mxM2opt);
		//cout << "\nMPC_Matlab_T::update(): end of part 4.1";
		for(i=0;i<Step_num;i++)
		{
			MPC_diag.Hopt[i]  = pHopt[i];
			MPC_diag.Dopt[i]  = pDopt[i];
			MPC_diag.R2opt[i] = pR2opt[i];
			MPC_diag.varR2opt[i] = pVarR2opt[i];
			MPC_diag.M2opt[i] = pM2opt[i];
		}
		//cout << "\nMPC_Matlab_T::update(): end of part 4.2";
		engEvalString(ep,"M2_0 = info.M2_0");
		mxM2_0 = engGetVariable(ep,"M2_0");
		pM2_0 = mxGetPr(mxM2_0);
		MPC_diag.M2_0 = pM2_0[0];		

		time += t_int_ctrl;

		// 5. Write to log file --------------------------------------------
		if(t_idx<t_node)
		{
			W_sum[t_idx] += dep_rate;
			W2_sum[t_idx] +=  dep_rate*dep_rate;
			T_sum[t_idx] += pT[0];
			T2_sum[t_idx] += pT[0]*pT[0];
		}

		if(Detail ==1)
			log_detail();
		// --------------------------------------------------------------
		t_idx++;

		mxDestroyArray(mxHopt);
		mxDestroyArray(mxDopt);
		mxDestroyArray(mxR2opt);
		mxDestroyArray(mxVarR2opt);
		mxDestroyArray(mxM2opt);
		mxDestroyArray(mxM2_0);
		//cout << "\nLeave MPC_Matlab_T::update()";
		return T;
	}
	catch(const char *s) 
	{
		cerr << "\nError at MPC_Matlab_T::update()";
		cerr << "\n" << MatlabOutputBuffer << "\n";
		cerr << "\n" << s;
	}
}
// ----------------------------------------------------------------------
double MPC_Matlab_T::update2(const double time,const lattice_state iLState)
{
	return 0;
}
// ----------------------------------------------------------------------
void MPC_Matlab_T::reset()
{
	ht		= 0.0;
	R2      = 0.0;
	g_dens	= 0.0;	
	dep_rate	= 0.2;	
	T           = T_ini;
	time		= 0.0;
	t_idx		= 0;
	
	W_sum[t_idx]  += dep_rate;
	W2_sum[t_idx] += dep_rate*dep_rate;
	T_sum[t_idx]  += T;
	T2_sum[t_idx] += T*T;

	t_idx ++;
	Repeated += 1;

	engEvalString(ep,"State.h = 0");
	engEvalString(ep,"State.rho = 0");
	engEvalString(ep,"State.meanAlpha2= zeros(1,Settings.m)");
	engEvalString(ep,"State.meanBeta2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.meanR2    = 0");
	engEvalString(ep,"State.varAlpha2 = zeros(1,Settings.m)");
	engEvalString(ep,"State.varBeta2  = zeros(1,Settings.m)");
	engEvalString(ep,"State.varR2     = 0");
	engEvalString(ep,"State.meanM2    = 0");
	engEvalString(ep,"T = 500");
}